package net.crazyadam

import net.crazyadam.Constant._
import math._

/**
 *
 * @author joseph
 * @since 6/17/12 10:41 PM
 */

class Path {
  var fullPath:List[Section]=Nil

  def addPoint(p:Point){
    //初始沒有數據時，新建Section。有數據後，但每次夾角大於閥值，則新建Section
    //&&angleGreaterThan(p.preTheta,threshold * ratio )
     if(fullPath.length==0||(angleGreaterThan(p.theta,fullPath.head,Constant.threshold)&&
       angleGreaterThan(p.preTheta,fullPath.head,Constant.threshold*Constant.ratio )))  {
       val s=new Section
       p.valid=false //每一Section開始一個Point均不計算
       s.addPoint(p)
       fullPath ::=s     //新建含單個元素p的Section。
       //開啓新一段的記錄，將總和置零。
//    println(path.length)
     }
    else{
       //夾角大於約閥值，添加p節點到最新一個list
       fullPath.head.addPoint(p)
     }
    //每次增加一個點，都累加。
  }
  def path=this.fullPath

  /**
   *
   * @param theta
   * @param section specific section
   * @return true if theta > threshold
   */
  def angleGreaterThan(theta:Double,section:Section,threshold:Double):Boolean={
    //角度相差越大，cos值相減差越大。
    acos(cos(theta-section.averageTheta)) > threshold
  }
  def mergeSections:List[Section] ={
    var list:List[Section]=Nil
    var currentSection:Section=null
    fullPath.reverse.foreach(s=>{
       if (null==currentSection){
         currentSection=s
         list::=currentSection
       }else{
          if (angleGreaterThan(s.averageTheta,currentSection,Constant.threshold)){
//            println("前"+currentSection.averageTheta+"後"+s.averageTheta)
             currentSection=s
            list::=currentSection
          }else{
            currentSection.merge(s)
          }
       }

    })
    fullPath=list
    fullPath
  }
}
